#include <ESP8266WiFi.h>
#include "CarCtrl.h"

CarCtrl::CarCtrl(int PIN_FL_EN, int PIN_FR_EN, int PIN_BL_EN, int PIN_BR_EN, int PIN_Data, int PIN_Latch, int PIN_Clock)
	: mPE_FL(PIN_FL_EN),	mPE_FR(PIN_FR_EN),	mPE_BL(PIN_BL_EN),	mPE_BR(PIN_BR_EN),	
	  mPLatch(PIN_Latch),	mPClock(PIN_Clock),	mPData(PIN_Data), 	mSpeed( 50 ), mStatus(0)
{}

CarCtrl::~CarCtrl(){};

void CarCtrl::SetDirSpeed(int dir, int speed)
{
	mSpeed = speed;
	switch (dir)
	{
	case CAR_DIR_UP:	Forward();	break;
	case CAR_DIR_DOWN:	Back();		break;
	case CAR_DIR_LEFT:	TurnLeft();	break;
	case CAR_DIR_RIGHT:	TurnRight(); break;
	case CAR_DIR_NONE:  Relase();  break;
	default:
		break;
	}
}

void CarCtrl::Start()
{
  pinMode(mPData,  OUTPUT);
  pinMode(mPLatch, OUTPUT);
  pinMode(mPClock, OUTPUT);

  pinMode(mPE_FL, OUTPUT);
  pinMode(mPE_FR, OUTPUT);
  pinMode(mPE_BL, OUTPUT);
  pinMode(mPE_BR, OUTPUT);

  mStatus = 0;
  digitalWrite(mPLatch, HIGH);
  analogWriteRange(255); 
}

void CarCtrl::SetStatus(String info) // FL(speed,dir);FR(speed,dir);BL(speed,dir);BR(speed,dir)
{	
	mStatus = 0;

	int si = 0;
	int ei = 0;

	info.trim();
	ei = info.indexOf(";", si);
	SetMotorStatus(info.substring(si, ei));
	si = ei + 1;
	ei = info.indexOf(";", si);
	SetMotorStatus(info.substring(si, ei));
	si = ei + 1;
	ei = info.indexOf(";", si);
	SetMotorStatus(info.substring(si, ei));
	si = ei + 1;
	SetMotorStatus(info.substring(si));	

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, mStatus);
  	digitalWrite(mPLatch, HIGH);
  	//Serial.println(mStatus);
}
		
void CarCtrl::SetMotorStatus(String status)//FL(speed,dir)
{


	int speed = status.substring(status.indexOf('(') + 1, status.indexOf(',')).toInt();
	int dir = status.substring(status.indexOf(',') + 1, status.indexOf(')')).toInt();
	switch( (status[0] == 'F' ? 0x10 : 0x00) + (status[1] == 'L' ? 0x01 : 0x00))
	{
	case 0x11: analogWrite(mPE_FL, speed); mStatus |= (dir == 1 ? 0x01 : 0x02); Serial.println( "FL enable"); break;
	case 0x10: analogWrite(mPE_FR, speed); mStatus |= (dir == 1 ? 0x04 : 0x08); break;
	case 0x01: analogWrite(mPE_BL, speed); mStatus |= (dir == 1 ? 0x10 : 0x20); break;
	case 0x00: analogWrite(mPE_BR, speed); mStatus |= (dir == 1 ? 0x40 : 0x80); break;
	}
	//Serial.print(status); Serial.print(speed); Serial.print(",");Serial.print(dir);Serial.print(",");Serial.println(mStatus);
}

void CarCtrl::Forward()
{
	analogWrite(mPE_FL, mSpeed);
	analogWrite(mPE_FR, mSpeed);
	analogWrite(mPE_BL, mSpeed);
	analogWrite(mPE_BR, mSpeed);

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, 0x55);
  	digitalWrite(mPLatch, HIGH);
}

void CarCtrl::Back()
{
	analogWrite(mPE_FL, mSpeed);
	analogWrite(mPE_FR, mSpeed);
	analogWrite(mPE_BL, mSpeed);
	analogWrite(mPE_BR, mSpeed);

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, 0xAA);
  	digitalWrite(mPLatch, HIGH);
}

void CarCtrl::Relase()
{
	digitalWrite(mPE_FL, LOW);
	digitalWrite(mPE_FR, LOW);
	digitalWrite(mPE_BL, LOW);
	digitalWrite(mPE_BR, LOW);

	digitalWrite(mPLatch, LOW);
	shiftOut(mPData, mPClock, MSBFIRST, 0x00);
	digitalWrite(mPLatch, HIGH);
}
void CarCtrl::Brake()
{
	digitalWrite(mPE_FL, HIGH);
	digitalWrite(mPE_FR, HIGH);
	digitalWrite(mPE_BL, HIGH);
	digitalWrite(mPE_BR, HIGH);

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, 0xFF);
  	digitalWrite(mPLatch, HIGH);	
}
void CarCtrl::TurnLeft()
{
	digitalWrite(mPE_FL, 100);
	digitalWrite(mPE_FR, 250);
	digitalWrite(mPE_BL, 0);
	digitalWrite(mPE_BR, 250);

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, 0x55);
  	digitalWrite(mPLatch, HIGH);	
}
void CarCtrl::TurnRight()
{
	digitalWrite(mPE_FL, 250);
	digitalWrite(mPE_FR, 100);
	digitalWrite(mPE_BL, 250);
	digitalWrite(mPE_BR, 0);

	digitalWrite(mPLatch, LOW);
  	shiftOut(mPData, mPClock, MSBFIRST, 0x55);
  	digitalWrite(mPLatch, HIGH);	
}